#include "armor_tracker/armor_tracker_node.hpp"

// STD
#include <memory>
#include <vector>

namespace rm_aim
{

/**
 * @brief 装甲板追踪节点类
 * @note 负责订阅装甲板检测数据、坐标转换、调用Tracker核心逻辑、发布追踪结果
 */
ArmorTrackerNode::ArmorTrackerNode(const rclcpp::NodeOptions & options)
: Node("armor_tracker_node", options)
{
  RCLCPP_INFO(this->get_logger(), "Starting ArmorTrackerNode!");

  // 1. 声明并获取核心参数
  // 装甲板在XOY平面的最大允许距离（过滤过远的无效目标）
  max_armor_distance_ = this->declare_parameter("max_armor_distance", 10.0);
  // Tracker匹配参数（最大距离、最大偏航角差）
  double max_match_distance = this->declare_parameter("tracker.max_match_distance", 0.15);
  double max_match_yaw_diff = this->declare_parameter("tracker.max_match_yaw_diff", 1.0);
  // Tracker状态阈值（稳定追踪所需连续匹配次数）
  general_tracking_thres_ = this->declare_parameter("tracker.tracking_thres", 5);
  outpost_tracking_thres_ = this->declare_parameter("tracker.tracking_thres_outpost", 10);
  // 目标丢失时间阈值（超过该时间判定为kLost）
  lost_time_thres_ = this->declare_parameter("tracker.lost_time_thres", 0.3);
  // EKF过程噪声与观测噪声参数
  general_sigma_q_xyz_ = this->declare_parameter("ekf.sigma2_q_xyz", 20.0);
  general_sigma_q_yaw_ = this->declare_parameter("ekf.sigma2_q_yaw", 100.0);
  general_sigma_q_r_ = this->declare_parameter("ekf.sigma2_q_r", 800.0);
  general_r_xyz_factor_ = this->declare_parameter("ekf.r_xyz_factor", 0.05);
  general_r_yaw_ = this->declare_parameter("ekf.r_yaw", 0.02);
  // 前哨站模式参数（命名空间ekf.outpost）
  outpost_sigma_q_xyz_ = this->declare_parameter("ekf.outpost.sigma2_q_xyz", 5.0);    
  outpost_sigma_q_yaw_ = this->declare_parameter("ekf.outpost.sigma2_q_yaw", 20.0);
  outpost_sigma_q_r_ = this->declare_parameter("ekf.outpost.sigma2_q_r", 10.0);
  outpost_r_xyz_factor_ = this->declare_parameter("ekf.outpost.r_xyz_factor", 0.02);
  outpost_r_yaw_ = this->declare_parameter("ekf.outpost.r_yaw", 0.01);

  enable_debug_pub_ = this->declare_parameter("enable_debug_pub", false);

  // 2. 初始化Tracker对象
  tracker_ = std::make_unique<ArmorTracker>(max_match_distance, max_match_yaw_diff);

  // 3. 创建重置追踪器的服务（用于手动重置追踪状态）
  reset_tracker_srv_ = this->create_service<std_srvs::srv::Trigger>(
    "/tracker/reset",
    [this](const std_srvs::srv::Trigger::Request::SharedPtr,
           std_srvs::srv::Trigger::Response::SharedPtr response) {
      tracker_->setTrackingState(TrackingState::kLost);// 重置为丢失状态
      response->success = true;
      RCLCPP_INFO(this->get_logger(), "ArmorTracker reset successfully!");
    });

  // 4. 初始化TF2相关组件（用于装甲板坐标从图像帧转换到世界帧）
  tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
  // 创建定时器接口（避免TF2等待转换时抛出异常）
  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
    this->get_node_base_interface(), this->get_node_timers_interface());
  tf2_buffer_->setCreateTimerInterface(timer_interface);
  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

  // 5. 订阅装甲板检测数据（带TF2消息过滤，确保坐标转换可用）
  target_frame_ = this->declare_parameter("target_frame", "odom");  // 目标世界坐标系
  armors_sub_.subscribe(this, "/hero_vision/armors", rmw_qos_profile_sensor_data);
  // TF2过滤器：等待目标坐标系可用后再触发回调
  tf2_filter_ = std::make_shared<tf2_filter>(
    armors_sub_, *tf2_buffer_, target_frame_, 50,  // 队列大小10
    this->get_node_logging_interface(),
    this->get_node_clock_interface(),
    std::chrono::duration<int>(1));  // 超时时间1秒
  // 注册装甲板数据回调函数
  tf2_filter_->registerCallback(&ArmorTrackerNode::armorsCallback, this);

  // 6. 创建发布者（追踪调试信息、核心目标结果）
  if (enable_debug_pub_)
  {
    info_pub_ = this->create_publisher<aim_interfaces::msg::TrackerInfo>(
    "/tracker/info", 10);  // 调试用：位置偏差、偏航角偏差等
  }
  target_pub_ = this->create_publisher<aim_interfaces::msg::Target>(
    "/tracker/target", rclcpp::SensorDataQoS());  // 核心输出：目标状态、速度等
}

void ArmorTrackerNode::armorsCallback(const aim_interfaces::msg::Armors::SharedPtr armors_msg)
{
  // 步骤1：将所有装甲板的坐标从图像帧转换到目标世界帧（如odom）
  for (auto & armor : armors_msg->armors) {
    geometry_msgs::msg::PoseStamped armor_pose_in_image;
    armor_pose_in_image.header = armors_msg->header;
    armor_pose_in_image.pose = armor.pose;

    try {
      // 执行坐标转换，覆盖原装甲板姿态
      armor.pose = tf2_buffer_->transform(armor_pose_in_image, target_frame_).pose;
    } catch (const tf2::ExtrapolationException & ex) {
      RCLCPP_ERROR(this->get_logger(), "TF transform failed: %s", ex.what());
      return;  // 转换失败时直接返回，不处理该帧数据
    }
  }

  // 步骤2：过滤异常装甲板（高度超限、距离超限）
  armors_msg->armors.erase(
    std::remove_if(
      armors_msg->armors.begin(), armors_msg->armors.end(),
      [this](const aim_interfaces::msg::Armor & armor) {
        // 条件1：Z轴高度超过1.2米（排除地面外无效目标）
        // 条件2：XOY平面距离超过max_armor_distance_（排除过远目标）
        return abs(armor.pose.position.z) > 1.2 ||
               Eigen::Vector2d(armor.pose.position.x, armor.pose.position.y).norm() > max_armor_distance_;
      }),
    armors_msg->armors.end());

  // 步骤3：初始化输出消息
  aim_interfaces::msg::TrackerInfo info_msg;    // 调试信息消息
  aim_interfaces::msg::Target target_msg;       // 核心目标消息
  const rclcpp::Time current_time = armors_msg->header.stamp;
  // 设置目标消息的时间戳和坐标系（与输入一致）
  target_msg.header.stamp = current_time;
  target_msg.header.frame_id = target_frame_;

  // 步骤4：根据追踪状态更新Tracker
  if (tracker_->getTrackingState() == TrackingState::kLost) {
    // 状态1：目标丢失→初始化追踪（从检测结果中选择初始目标）
    tracker_->initialize(armors_msg);
    // 初始化后立即判断是否为前哨站，设置正确参数
    bool is_outpost_init = (tracker_->tracked_armor_count == ArmorCount::kOutpost3);
    if (is_outpost_init) {
    tracker_->setEKFParams(outpost_sigma_q_xyz_, outpost_sigma_q_yaw_, outpost_sigma_q_r_, outpost_r_xyz_factor_, outpost_r_yaw_);
    tracker_->tracking_thres = outpost_tracking_thres_;
    }else {
      tracker_->setEKFParams(general_sigma_q_xyz_, general_sigma_q_yaw_, general_sigma_q_r_, general_r_xyz_factor_, general_r_yaw_);
      tracker_->tracking_thres = general_tracking_thres_;
    }
    target_msg.tracking = false;  // 未进入稳定追踪
  } else {
    bool is_outpost = (tracker_->tracked_armor_count == ArmorCount::kOutpost3);
    if (is_outpost) {
      tracker_->setEKFParams(outpost_sigma_q_xyz_, outpost_sigma_q_yaw_, outpost_sigma_q_r_, outpost_r_xyz_factor_, outpost_r_yaw_);
      tracker_->tracking_thres = outpost_tracking_thres_;
    }else {
      tracker_->setEKFParams(general_sigma_q_xyz_, general_sigma_q_yaw_, general_sigma_q_r_, general_r_xyz_factor_, general_r_yaw_);
      tracker_->tracking_thres = general_tracking_thres_;
    }
    // 状态2：非丢失→计算时间间隔（用于EKF预测）
    dt_ = (current_time - last_time_).seconds();
    // 设置丢失阈值（根据时间间隔换算为帧数）
    tracker_->lost_thres = static_cast<int>(lost_time_thres_ / dt_);
    // 执行追踪更新（EKF预测→装甲板匹配→EKF更新）
    tracker_->updateTracking(armors_msg);

    // 发布调试信息（位置偏差、偏航角偏差、当前观测值）
    if (enable_debug_pub_ && info_pub_) {
    info_msg.position_diff = tracker_->info_position_diff;
    info_msg.yaw_diff = tracker_->info_yaw_diff;
    info_msg.position.x = tracker_->measurement(0);  // 观测值X
    info_msg.position.y = tracker_->measurement(1);  // 观测值Y
    info_msg.position.z = tracker_->measurement(2);  // 观测值Z
    info_msg.yaw = tracker_->measurement(3);         // 观测值偏航角
    info_pub_->publish(info_msg);
    }
    // 根据当前追踪状态设置目标消息
  switch (tracker_->getTrackingState()) {
      case TrackingState::kDetecting:
        // 检测中（未稳定匹配）→ 标记为未追踪
        target_msg.tracking = false;
        break;
      case TrackingState::kTracking:
      case TrackingState::kTempLost:
        // 稳定追踪/暂时丢失→ 标记为追踪中，并填充目标状态
        target_msg.tracking = true;
        {
          const Eigen::VectorXd & target_state = tracker_->target_state;  // EKF输出的9维状态
          // 目标标识信息
          target_msg.id = tracker_->tracked_armor_id;
          // 装甲板数量（转换为枚举对应的数值，如kNormal4→4）
          if (tracker_->tracked_armor_count == ArmorCount::kNormal4) {
            target_msg.armors_num = 4;
          } else if (tracker_->tracked_armor_count == ArmorCount::kBalance2) {
            target_msg.armors_num = 2;
          } else if (tracker_->tracked_armor_count == ArmorCount::kOutpost3) {
            target_msg.armors_num = 3;
          }
          // 位置信息（x_c, y_c, z_a）
          target_msg.position.x = target_state(0);
          target_msg.position.y = target_state(2);
          target_msg.position.z = target_state(4);
          // 速度信息（v_xc, v_yc, v_za）
          target_msg.velocity.x = target_state(1);
          target_msg.velocity.y = target_state(3);
          target_msg.velocity.z = target_state(5);
          // 姿态与角速度（yaw, v_yaw）
          target_msg.yaw = target_state(6);
          target_msg.v_yaw = target_state(7);
          // 装甲板半径（r）与（radius_1, radius_2）
          target_msg.radius_1 = target_state(8);
          target_msg.radius_2 = tracker_->another_r;
          //高度差
          target_msg.dz = tracker_->dz;
        }
        break;
      case TrackingState::kLost:
        // 目标丢失状态
        target_msg.tracking = false;
        break;
      default:
        target_msg.tracking = false;
        break;
    }
  }

  // 步骤5：更新上一帧时间戳，并发布核心目标消息
  last_time_ = current_time;
  target_pub_->publish(target_msg);
}

}  // namespace rm_aim

#include "rclcpp_components/register_node_macro.hpp"

// 注册节点到ROS组件管理器（允许通过组件加载方式启动）
RCLCPP_COMPONENTS_REGISTER_NODE(rm_aim::ArmorTrackerNode)